import sensor, image, time, math
import pyb
from pyb import UART
import ulab as np


# LED状态指示初始化
pyb.LED(1).on()  # 红色LED指示电源
pyb.LED(2).on()  # 绿色LED指示摄像头状态
pyb.LED(3).on()  # 蓝色LED指示通信状态

# #################### 常量定义 ####################
# 系统操作模式字典（状态机基础）
OPERATION_MODE = {
    'STANDBY': 0,                 # 待机模式
    'LINE_TRACKING': 1,           # 巡线模式
    'COLOR_DETECTION': 2,         # 物料颜色识别
    'COARSE_AREA_DETECTION': 3,   # 粗加工区定位
    'BUFFER_AREA_DETECTION': 4,   # 缓存区定位
    'QR_CODE_SCAN': 5             # 二维码扫描
}


# 命令映射表
COMMAND_MAP = {
    0x01: 'LINE_TRACKING',
    0x02: 'COLOR_DETECTION',
    0x03: 'COARSE_AREA_DETECTION',
    0x04: 'BUFFER_AREA_DETECTION',
    0x05: 'QR_CODE_SCAN',
    0x00: 'STANDBY'
}

# 颜色阈值设置
COLOR_THRESHOLDS = {
    'red': [(30, 100, 15, 127, 15, 127)],
    #'green': [(0, 80, -70, -10, -0, 30)],
    'green': [(32, 87, -67, -28, -52, 60)],
    #'blue': [(0, 30, 0, 64, -128, -20)],
    #'blue' : [(100, 28, -99, 74, -128, -15)],
    #'blue' : [(30, 70, -20, 20, -70, -20)],
    'blue' : [(51, 71, -27, -3, -55, -25)],
    'line_tracking': [(0, 30, 0, 64, -128, -20)],  # 黑线阈值
    'green_coarse_area': [(19, 95, -13, -40, -74, 58)]  # 粗加工区绿色标记
}

# 检测参数
DETECTION_PARAMS = {
    'line_regression': {
        'x_stride': 2,
        'y_stride': 2,
        'threshold': 1000,
        'theta_margin': 25,
        'rho_margin': 25
    },
    'coarse_area_circle': {
        # 'r_min': 61,
        # 'r_max': 67,
        'r_min': 50,
        'r_max': 95,
        'samples_needed': 2,
        'area_threshold': 12000
    },
    'buffer_area_circle': {
        # 'r_min': 61,
        # 'r_max': 67,
        'r_min': 50,
        'r_max': 95,
        'samples_needed': 2,
        'area_threshold': 12000
    }
}

# QR码前缀
QR_CODE_HEADER = "QR:"

# #################### 初始化设置 ####################
# 摄像头初始化（图像采集核心配置）
sensor.reset()  # 重置摄像头硬件
sensor.set_pixformat(sensor.RGB565)  # 设置彩色模式
sensor.set_framesize(sensor.QQVGA)    # 分辨率320x240
sensor.skip_frames(time=2000)       # 等待感光元件稳定
sensor.set_auto_gain(False)         # 固定增益防止亮度波动
sensor.set_auto_whitebal(False)     # 关闭自动白平衡
sensor.set_auto_exposure(False, exposure_us=10000)  # 固定曝光

# UART初始化
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)

# 全局变量
#current_operation = OPERATION_MODE['STANDBY']
line_x_samples = [0] * 15
line_y_samples = [0] * 15
sample_counter = 0

# #################### 功能函数 ####################
def calculate_median(values, count):
    """计算中值滤波"""
    return np.median(np.array(values[:count]))

# ******************** 发送函数 ********************
def send_data(*payload):
    """
    简化版数据发送函数
    参数格式：任意数量的整型数据 (0-255)
    示例：send_data(0x01, 120, 180) 发送3个字节 01 78 B4
    """
    for byte in payload:
        uart.writechar(byte)  # 逐个字节发送

    # 可选：添加延时保证数据包完整性（波特率115200时1字节约87μs）
    # pyb.udelay(100 * len(payload))  # 每字节额外延时100μs

# ******************** 接收解析函数 ********************
def parse_command():
    """
    即时读取串口缓冲区
    返回格式：{'valid': bool, 'command': str, 'data': list}
    """
    if uart.any():
        # 读取所有可用字节
        raw_data = uart.read()
        print("Raw UART Data:", [hex(b) for b in raw_data])

        # 转换为可操作的字节数组
        data_bytes = bytearray(raw_data)
        result = {
            'valid': False,
            'command': 'UNKNOWN',
            'data': []
        }

        # 至少包含1个命令字节
        if len(data_bytes) >= 1:
            result['valid'] = True
            result['command'] = COMMAND_MAP.get(data_bytes[0], 'UNKNOWN')
            result['data'] = list(data_bytes[1:])  # 命令后的数据

        return result
    return {'valid': False, 'command': 'UNKNOWN', 'data': []}


# #################### 主循环 ####################
current_operation = OPERATION_MODE['COLOR_DETECTION']
#current_operation = OPERATION_MODE['COARSE_AREA_DETECTION']
#send_data(0x01, 120, 180)

clock = time.clock()
while True:
    clock.tick()
    img = sensor.snapshot()

    # 检查并处理UART指令
    cmd_result = parse_command()
    if cmd_result['valid']:
        command_str = cmd_result['command']
        if command_str in OPERATION_MODE:
            current_operation = OPERATION_MODE[command_str]
            print(f"切换到{command_str}模式")


    if current_operation == OPERATION_MODE['COLOR_DETECTION']:
        # ########## 颜色分拣模式 ##########
        # 初始化检测参数
        img.draw_rectangle(60, 40, 200, 160, color=(255,0,0))

        detection_enabled = True
        min_blob_area = 10000  # 最小有效物料面积(根据实际尺寸调整)
        max_blob_ratio = 2.5  # 宽高比过滤(排除细长干扰物)
        color_priority = ['blue', 'green', 'red']  # 检测优先级

        # 创建颜色检测器字典
        color_detectors = {
            'red': {
                'threshold': COLOR_THRESHOLDS['red'],
                'code': 0x02,
                'marker_color': (255, 0, 0)  # 红色标记
            },
            'green': {
                'threshold': COLOR_THRESHOLDS['green'],
                'code': 0x03,
                'marker_color': (0, 255, 0)  # 绿色标记
            },
            'blue': {
                'threshold': COLOR_THRESHOLDS['blue'],
                'code': 0x04,
                'marker_color': (0, 0, 255)  # 蓝色标记
            }
        }

        detected_objects = []  # 存储检测到的物料信息

        for color in color_priority:
            # 获取当前颜色的检测参数
            detector = color_detectors[color]

            #查找色块（添加ROI限制和合并参数）
            blobs = img.find_blobs(
                COLOR_THRESHOLDS['blue'],
                roi=(60, 40, 200, 160),  # 限定中央检测区域
                area_threshold=min_blob_area,
                merge=True,  # 合并相邻色块
                margin=20,   # 合并边距
                pixels_threshold=100
            )

            # 过滤有效物料
            valid_blobs = [
                b for b in blobs
                if (b.w() / b.h() < max_blob_ratio) and
                   (b.h() / b.w() < max_blob_ratio)
            ]


            if valid_blobs:
                # 选择面积最大的作为主要目标
                main_blob = max(valid_blobs, key=lambda b: b.area())

                # 绘制检测图形
                img.draw_rectangle(main_blob.rect(), color=detector['marker_color'])
                img.draw_cross(main_blob.cx(), main_blob.cy(),
                              color=detector['marker_color'])

                # 记录检测结果
                detected_objects.append({
                    'color': color,
                    'x': main_blob.cx(),
                    'y': main_blob.cy(),
                    'area': main_blob.area()
                })

                # 发送坐标信息(添加比例换算)
                send_x = int(main_blob.cx() * 255 / img.width())   # 归一化到0-255
                send_y = int(main_blob.cy() * 255 / img.height())
                send_data(detector['code'], send_x, send_y)
                print(detector['code'], send_x, send_y)

                # 优先处理最高级颜色
                break

        # 多颜色同屏处理（如果允许同时处理）
        if len(detected_objects) > 1:
            # 按面积排序选择最大目标
            main_target = max(detected_objects, key=lambda x: x['area'])
            img.draw_string(main_target['x'], main_target['y']-20,
                           main_target['color'].upper(),
                           color=(255,255,255))

            # 获取当前目标对应的颜色检测器
            correct_detector = color_detectors[main_target['color']]

            # 发送综合数据包
            send_data(0x0F,
                      correct_detector['code'],  # 正确颜色编码
                      main_target['x'],
                      main_target['y'])



    elif current_operation == OPERATION_MODE['COARSE_AREA_DETECTION']:
        # ########## 粗加工区检测 ##########

        img.binary(COLOR_THRESHOLDS['green_coarse_area'])
        circles = img.find_circles(
            threshold=DETECTION_PARAMS['coarse_area_circle']['area_threshold'],
            r_min=DETECTION_PARAMS['coarse_area_circle']['r_min'],
            r_max=DETECTION_PARAMS['coarse_area_circle']['r_max']
        )

        for circle in circles:
            img.draw_circle(circle.x(), circle.y(), circle.r(), color=(255, 0, 0))
            if sample_counter < DETECTION_PARAMS['coarse_area_circle']['samples_needed']:
                line_x_samples[sample_counter] = circle.x()
                line_y_samples[sample_counter] = circle.y()
                sample_counter += 1
                # print(0x03, line_x_samples, line_y_samples)

            else:
                avg_x = sum(line_x_samples) // sample_counter
                avg_y = sum(line_y_samples) // sample_counter
                send_data(0x03, avg_x, avg_y)
                sample_counter = 0
            print(0x03, avg_x, avg_y)


    # 帧率显示
    img.draw_string(0, 0, "FPS:%.1f" % clock.fps(), color=(255, 0, 0))
    img.draw_string(0, 15, f"Mode: {list(OPERATION_MODE.keys())[current_operation]}",
                  color=(255, 0, 0))
# send_data(detector['code'], send_x, send_y)
